#!/usr/bin/env python3
import cv2
import rclpy
from rclpy.node import Node
import cv_bridge
from sensor_msgs.msg import Image
import os


class CameraReader(Node):
    def __init__(self):
        super().__init__("camera_reader")
        self.declare_parameter("device", "undefine")
        self.declare_parameter("width", 2560)
        self.declare_parameter("height", 720)
        self.declare_parameter("fps", 60)
        self.declare_parameter("show_image", False)
        self.declare_parameter("flip_horizontal", False)
        self.declare_parameter("flip_vertical", False)
        device = self.get_parameter("device").get_parameter_value().string_value
        width = self.get_parameter("width").get_parameter_value().integer_value
        height = self.get_parameter("height").get_parameter_value().integer_value
        fps = self.get_parameter("fps").get_parameter_value().integer_value
        self.show = self.get_parameter("show_image").get_parameter_value().bool_value
        self.flip_horizontal = (
            self.get_parameter("flip_horizontal").get_parameter_value().bool_value
        )
        self.flip_vertical = (
            self.get_parameter("flip_vertical").get_parameter_value().bool_value
        )

        if self.flip_horizontal and self.flip_vertical:
            self.flip = -1
        elif self.flip_horizontal:
            self.flip = 1
        elif self.flip_vertical:
            self.flip = 0
        else:
            self.flip = -2

        if device != "undefine":
            self.cap = cv2.VideoCapture(device, cv2.CAP_V4L)
        else:
            self.cap = cv2.VideoCapture(0, cv2.CAP_V4L)
        if not self.cap.isOpened():
            self.get_logger().error("Failed to open camera")
            exit(1)

        self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc("M", "J", "P", "G"))
        # self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc("Y", "U", "Y", "2"))
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
        self.cap.set(cv2.CAP_PROP_FPS, fps)
        self.create_timer(1.0 / fps, self.read_frame_callback)
        self.pub_left = self.create_publisher(Image, "camera_image_left", 10)
        self.pub_right = self.create_publisher(Image, "camera_image_right", 10)

        if self.show:
            self.get_logger().info("Showing camera images")
        else:
            self.get_logger().info("Not showing camera images")

    def read_frame_callback(self):
        frame = self.read_frame()
        if frame is None:
            self.get_logger().error("Failed to read frame")
            return

        frame_left = frame[:, : (int)(frame.shape[1] / 2)]
        frame_right = frame[:, (int)(frame.shape[1] / 2) :]
        self.pub_left.publish(
            cv_bridge.CvBridge().cv2_to_imgmsg(frame_left, encoding="bgr8")
        )
        self.pub_right.publish(
            cv_bridge.CvBridge().cv2_to_imgmsg(frame_right, encoding="bgr8")
        )
        if self.show:
            cv2.imshow("frame_left", frame_left)
            cv2.imshow("frame_right", frame_right)
            if cv2.waitKey(1) & 0xFF == ord("c"):
                if not (
                    cv2.imwrite(
                        os.path.join(os.path.expanduser("~"), "图片/frame_left.jpg"),
                        frame_left,
                    )
                    and cv2.imwrite(
                        os.path.join(os.path.expanduser("~"), "图片/frame_right.jpg"),
                        frame_right,
                    )
                ):
                    self.get_logger().error("Failed to save image")
                else:
                    self.get_logger().info("Frame captured")

    def read_frame(self):
        ret, frame = self.cap.read()
        if ret:
            if self.flip != -2:
                frame = cv2.flip(frame, self.flip)
            return frame
        else:
            return None

    def __del__(self):
        self.cap.release()
        cv2.destroyAllWindows()

def main(args=None):
    rclpy.init(args=args)
    node = CameraReader()
    node.get_logger().info("Camera Reader node started")
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()
